#include "ePuck.h"
#include <stdio.h>
#include <stdarg.h>

namespace ePuck
{
  Device::Device( void )
  : mpConnectionHandle( NULL )
  , mConnected( false )
  {
  }

  Device::~Device( void )
  {
    if( NULL != mpConnectionHandle )
    {
      delete mpConnectionHandle;
      mpConnectionHandle = NULL;
    }
  }

  EResult 
  Device::Connect( const std::string& rDevice )
  {
    EResult Result = EResultOk;

    if( EResultOk == Result )
    {
      mpConnectionHandle = fopen( rDevice.c_str(), "r+" );
      Result = 
      ( 
        NULL != mpConnectionHandle 
        ? EResultOk 
        : EResultUnableToOpenDevice 
      );
    }
      
    if( EResultOk == Result )
    {
      mConnected = true;
      // Flush the UART buffer of the ePuck
      fprintf( mpConnectionHandle, "v\n" );
    }

    return Result;
  }
  
  EResult
  Device::Move( const signed Left, const signed Right )
  {
    EResult Result  = EResultOk;
    signed left     = Left;
    signed right    = Right; 
    
    // Scale input values without warning caller
    if( EResultOk == Result )
    {
      LimitMotorSpeed( left );
      LimitMotorSpeed( right );
    }

    // Check if device is connected
    if( EResultOk == Result )
    {
      Result = CheckInitialization( );
    }

    // Send move command
    if( EResultOk == Result )
    {
      Result = SendCommand( "D,%d,%d\n", 
        left, right );
    }

    return Result;
  }

  EResult 
  Device::Sound( const ESound Sound )
  {
    EResult Result  = EResultOk;

    // Check if device is connected
    if( EResultOk == Result )
    {
      Result = CheckInitialization( );
    }

    // Send sound command
    if( EResultOk == Result )
    {
      Result = SendCommand( "T,%d\n", 
        static_cast<unsigned>( Sound ) );
    }

    return Result;
  }

  EResult 
  Device::Reset( void )
  {
    EResult Result  = EResultOk;

    // Check if device is connected
    if( EResultOk == Result )
    {
      Result = CheckInitialization( );
    }

    // Send sound command
    if( EResultOk == Result )
    {
      Result = SendCommand( "R\n" );
    }

    return Result;
  }
  
  EResult 
  Device::Stop( void )
  {
    EResult Result  = EResultOk;

    // Check if device is connected
    if( EResultOk == Result )
    {
      Result = CheckInitialization( );
    }

    // Send sound command
    if( EResultOk == Result )
    {
      Result = SendCommand( "S\n" );
    }

    return Result;
  }
  
  EResult 
  Device::SetLed(   const ELedId Led, const ELedState State )
  {
    EResult Result  = EResultOk;

    // Check if device is connected
    if( EResultOk == Result )
    {
      Result = CheckInitialization( );
    }

    // Send sound command
    if( EResultOk == Result )
    {
      Result = SendCommand( "%c,%d\n",
        static_cast<char>( Led ),
        static_cast<unsigned>( State ) );
    }

    return Result;
  }
  
  EResult 
  Device::GetProximity( SProximity& rProximity )
  {
    EResult Result = EResultOk;

    // Check if device is connected
    if( EResultOk == Result )
    {
      Result = CheckInitialization( );
    }
    char temp[1024];
    fread((void*)temp, sizeof(char), 1024, mpConnectionHandle );

    // Send proximity command
    if( EResultOk == Result )
    {
      Result = SendCommand( "N\n" );
      sleep( 2 );
    }


    if( EResultOk == Result )
    {
      for( unsigned i = 0; i < 200 && temp[0] != 'n'; i++ )
      {
        // Read single line from bluetooth driver
//        Result = SendCommand( "N\n" );
  //      sleep( 2 );
        fscanf( mpConnectionHandle, "%s\r\n", temp );
        printf( "%d: read: %s\n", i, temp );
      }

      if( temp[0] != 'n' )
      {
        printf( "temp[0]=%c\n", temp[0] );
        Result = EResultFail;
      }
    }

    if( EResultOk == Result )
    {
      printf( "Filling the bitch\n" );
      scanf
      ( 
        temp
      , "n,%u,%u,%u,%u,%u,%u,%u,%u"
      , &rProximity.S0
      , &rProximity.S1
      , &rProximity.S2
      , &rProximity.S3
      , &rProximity.S4
      , &rProximity.S5
      , &rProximity.S6
      , &rProximity.S7
      );
    }

    return Result;
  }
  
  
  void 
  Device::LimitMotorSpeed( signed& rValue )
  {
    rValue = rValue >  1024 ?  1024 : rValue;
    rValue = rValue < -1024 ? -1024 : rValue;
  }
  
  EResult
  Device::CheckInitialization( void ) 
  {
    EResult Result = EResultOk;

    Result = 
    ( 
      true == mConnected 
      ? EResultOk 
      : EResultDeviceNotConnected 
    );

    return Result;
  }

  EResult 
  Device::SendCommand( const char* format, ... )
  {
    EResult Result = EResultOk;
    int count = 0;

    va_list args;
    va_start( args, format );
    count = vfprintf( mpConnectionHandle, format, args );
    va_end( args );

    Result = 
    ( 
      0 < count 
      ? EResultOk 
      : EResultUnableToWriteToDevice 
    );

    return Result;
   
  }

  FILE*
  Device::GetHandle( void )
  {
    return mpConnectionHandle;
  }
}

